#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 1. 定义launch文件接收的参数变量
    bag_file = LaunchConfiguration('bag_file')
    point_cloud_topic = LaunchConfiguration('point_cloud_topic')
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    
    # 2. 声明需要从命令行传入的参数
    declare_bag_file_arg = DeclareLaunchArgument(
        'bag_file',
        default_value='/home/cheng/视频/shuju2/rosbag2_2025_10_16-12_11_26',
        description='Path to the ROS2 bag file'
    )
    
    declare_point_cloud_topic_arg = DeclareLaunchArgument(
        'point_cloud_topic',
        default_value='/livox/lidar',
        description='Point cloud topic name'
    )
    
    # 3. 启动ROS2 bag回放进程
    play_bag_cmd = ExecuteProcess(
        # -r 1.0 可以控制播放速率，--remap可以重映射话题
        cmd=['ros2', 'bag', 'play', bag_file, '--clock', '-l'], 
        output='screen'
    )
    
    # 4. 【新增】启动静态TF变换发布节点
    #    这个节点是新代码所必需的，它告诉系统"livox_frame"相对于"odom"坐标系的位置。
    #    我们假设雷达是固定的，所以发布一个静态（零偏移）的变换。
    #    参数: ['x', 'y', 'z', 'yaw', 'pitch', 'roll', 'parent_frame', 'child_frame']
    static_transform_publisher_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_odom_to_livox_publisher',
        arguments=['0', '0', '0', '0', '0', '0', 'odom', 'livox_frame']
    )
    # 注意：请确保 'livox_frame' 与您bag包中点云消息头里的 frame_id 完全一致！
    
    # 5. 启动你的点云处理节点
    lidar_processor_node = Node(
        package='lidar_processing', # 确保这是你的功能包名称
        executable='gaochengtu',   # 你的python脚本可执行文件名
        name='lidar_process',
        parameters=[
            {'use_sim_time': use_sim_time},
            {'input_cloud': point_cloud_topic}
        ],
        output='screen'
    )
    
    # 6. 返回launch描述，包含所有要执行的动作
    return LaunchDescription([
        declare_bag_file_arg,
        declare_point_cloud_topic_arg,
        play_bag_cmd,
        static_transform_publisher_node, # <-- 添加了新的TF节点
        lidar_processor_node
    ])